#include "Sphere.h"
#include <cmath>

Sphere::Sphere(dWorldID world, dSpaceID space, dReal radius, dReal mass)
{
	body = dBodyCreate(world);	
	geom = dCreateSphere(space, radius);

	dMass m;
	dMassSetSphere(&m,1.0f,radius);  	
	dMassAdjust(&m,mass);  
	dBodySetMass(body,&m);

	dGeomSetBody(geom,body);

	//Setup verticies (code is modified from Paul's Projects, BSD licence is maintained as follows)
	//////////////////////////////////////////////////////////////////////////////////////////	
	//	Copyright (c) 2006, Paul Baker
	//	Distributed under the New BSD Licence. (See accompanying file License.txt or copy at
	//	http://www.paulsprojects.net/NewBSDLicense.txt)
	//////////////////////////////////////////////////////////////////////////////////////////	



	//Top vertex
	Vertex sphereTop;
	sphereTop.position = cml::vector3d(0.0, radius, 0.0);
	sphereTop.normal = cml::vector3d(0.0, 1.0, 0.0);
	//sphereTop.color = cml::vector3f(1.0, 1.0, 1.0);
	vertices.push_back(sphereTop);


	GLuint stack, slice;
	//Middle ones
	for(stack = 1; stack < numStacks; ++stack)
	{
		double theta = M_PI * (double(stack) / numStacks);

		for(slice = 0; slice < numSlices; ++slice)
		{
			double phi=2 * M_PI * (double(slice) / numSlices);

			Vertex currentVertex;

			currentVertex.position = cml::vector3d(float(radius*sin(theta)*cos(phi)),
												   float(radius*cos(theta)),
												   float(radius*sin(theta)*sin(phi)));

			currentVertex.normal = cml::normalize(currentVertex.position);

			//currentVertex.color = cml::vector3f(((float)stack)/numStacks, ((float)stack)/numStacks, ((float)stack)/numStacks);

			vertices.push_back(currentVertex);
		}
		printf("\n"); //DEBUG
	}

	//Bottom vertex
	Vertex sphereBottom;
	sphereBottom.position = cml::vector3d(0.0, -radius, 0.0);
	sphereBottom.normal = cml::vector3d(0.0, -1.0, 0.0);
	//sphereBottom.color = cml::vector3f(0.0, 0.0, 0.0);
	vertices.push_back(sphereBottom);


	//Set up indicies
	//These are used by openGL to select in which order to pick vertices for drawing

	for(slice=0; slice<numSlices; ++slice)
	{
		indices.push_back(0);
		indices.push_back(1+slice);
		indices.push_back(1+(slice==numSlices-1 ? 0 : slice+1)); //Deals with last triangle
	}

	//Now do all but the last stack
	for(stack=1; stack<numStacks-1; ++stack)
	{
		for(GLuint slice=0; slice<numSlices; ++slice)
		{
			indices.push_back(1+(stack-1)*numSlices+slice);
			indices.push_back(1+stack*numSlices+slice);
			indices.push_back(1+(stack-1)*numSlices+(slice==numSlices-1 ? 0 : slice+1));

			indices.push_back(1+stack*numSlices+slice);
			indices.push_back(1+stack*numSlices+(slice==numSlices-1 ? 0 : slice+1));
			indices.push_back(1+(stack-1)*numSlices+(slice==numSlices-1 ? 0 : slice+1));
		}
	}

	//The final stack is a triangle fan
	for(GLuint slice=0; slice<numSlices; ++slice)
	{
		indices.push_back(1+(numStacks-2)*numSlices+slice);
		indices.push_back(1+(numStacks-1)*numSlices);
		indices.push_back(1+(numStacks-2)*numSlices+(slice==numSlices-1 ? 0 : slice+1));
	}

	//End modified Paul's code
}

void Sphere::renderInLocalFrame() const
{
	glColor3ubv(color);

	GLfloat mat_specular[] = {0.1f, 0.1f, 0.1f};
	glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);

	//Complex but nicer drawing code
	
	/*glEnableClientState(GL_VERTEX_ARRAY);
	glEnableClientState(GL_NORMAL_ARRAY);
	glEnableClientState(GL_COLOR_ARRAY);

	glVertexPointer(3, GL_FLOAT, sizeof(Vertex), &vertices[0].position);
	glNormalPointer(GL_FLOAT, sizeof(Vertex), &vertices[0].normal);
	glColorPointer(3, GL_FLOAT, sizeof(Vertex), &vertices[0].color);

	glDrawElements(GL_TRIANGLES, indices.size(), GL_UNSIGNED_INT, &indices[0]);

	glDisableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_NORMAL_ARRAY);
	glDisableClientState(GL_COLOR_ARRAY);*/
	

	//Less complex but a bit less nice
	
	dReal radius;	
	radius = dGeomSphereGetRadius(geom);	
	glScalef(radius, radius, radius);	
	glutSolidSphere(1.0f, numSlices, numStacks);
	
}
